#include <ros/ros.h>
#include "optitopo.h"
#include <string>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Core>
#include <geometry_msgs/PoseStamped.h>
#include <cstdlib>
#include <fstream>
#include "matplotlibcpp.h"
using namespace Eigen;
using namespace std;
namespace plt = matplotlibcpp;

int ctrl_rate = 30;
double last_t_gap = 0;//开始轨迹追踪的时间

//topo param
double T_exp=6;
double k_v_theta_max = 1.5;
double k_v_theta_min = 0.5;
double a_theta_max = 1.0;
double dis_min = 0.5;
double Resolution = 6;
double Coeff_Vel = 0.00005;

double pso_dt = 0.02; // 第i维度上的时间步长
double pso_wstart = 0.9;		// 第i维度上的起始权重系数
double pso_wend = 0.4;			// 第i维度上的终止权重系数
double pso_C1 = 1.49445;		// 第i维度上的加速度因子
double pso_C2 = 1.49445;		// 第i维度上的加速度因子
int pso_particle_num = 20;      // pso粒子个数
int pso_max_iter_num = 50;      // pso最大迭代次数
int dim = 4;

// tool func
template<typename T>
void readParam(ros::NodeHandle &nh, std::string param_name, T& loaded_param) {
    // template to read param from roslaunch
    const string& node_name = ros::this_node::getName();
    param_name = node_name + "/" + param_name;
    if (!nh.getParam(param_name, loaded_param)) {
        ROS_ERROR_STREAM("Failed to load " << param_name << ", use default value");
        //TODO:print default value
    }
    else{
        ROS_INFO_STREAM("Load " << param_name << " success");
        //TODO:print loaded value

    }
}


void loadRosParams(ros::NodeHandle &nh) {
    readParam<double>(nh, "T_exp", T_exp);
    readParam<double>(nh, "k_v_theta_max", k_v_theta_max);
    readParam<double>(nh, "k_v_theta_min", k_v_theta_min);
    readParam<double>(nh, "a_theta_max", a_theta_max);
    readParam<double>(nh, "dis_min", dis_min);
    readParam<double>(nh, "Resolution", Resolution);
    readParam<double>(nh, "Coeff_Vel", Coeff_Vel);

    readParam<double>(nh, "pso_dt", pso_dt);
    readParam<double>(nh, "pso_wstart", pso_wstart);
    readParam<double>(nh, "pso_wend", pso_wend);
    readParam<double>(nh, "pso_C1", pso_C1);
    readParam<double>(nh, "pso_C2", pso_C2);
    readParam<int>(nh, "pso_particle_num", pso_particle_num);
    readParam<int>(nh, "pso_max_iter_num", pso_max_iter_num);
}


int main (int argc, char** argv) {
    // ros init
    ros::init(argc, argv, "testopt", ros::init_options::AnonymousName);
    ros::NodeHandle nh;
    loadRosParams(nh);

    ros::Time PID_begin_time = ros::Time::now();
    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate ctrl_loop(ctrl_rate);

    geometry_msgs::PoseStamped exp_pose_1;
    geometry_msgs::PoseStamped exp_pose_2;
    geometry_msgs::PoseStamped exp_pose_3;
    geometry_msgs::PoseStamped exp_pose_4;

    ros::Publisher uav1_pub = nh.advertise<geometry_msgs::PoseStamped>("/uav1_info", 1);
    ros::Publisher uav2_pub = nh.advertise<geometry_msgs::PoseStamped>("/uav2_info", 1);
    ros::Publisher uav3_pub = nh.advertise<geometry_msgs::PoseStamped>("/uav3_info", 1);
    ros::Publisher uav4_pub = nh.advertise<geometry_msgs::PoseStamped>("/uav4_info", 1);

    optitopo SwarmOptiTopo(dim, pso_particle_num, pso_max_iter_num*2); //声明拓扑优化类
	SwarmOptiTopo.max_iter_num_ = pso_max_iter_num;	// 最大迭代次数
    for (int i = 0; i < SwarmOptiTopo.dim_;i++){
        SwarmOptiTopo.dt_[i] = pso_dt; // 第i维度上的时间步长
        SwarmOptiTopo.wstart_[i] = pso_wstart;		// 第i维度上的起始权重系数
        SwarmOptiTopo.wend_[i] = pso_wend;			// 第i维度上的终止权重系数
        SwarmOptiTopo.C1_[i] = pso_C1;		// 第i维度上的加速度因子
        SwarmOptiTopo.C2_[i] = pso_C2;		// 第i维度上的加速度因子
    }

    SwarmOptiTopo.T_exp_ = T_exp;
    SwarmOptiTopo.k_v_theta_max_ = k_v_theta_max;
    SwarmOptiTopo.k_v_theta_min_ = k_v_theta_min;
    SwarmOptiTopo.a_theta_max_ = a_theta_max;
    SwarmOptiTopo.dis_min_ = dis_min;
    SwarmOptiTopo.Resolution_ = Resolution;
    SwarmOptiTopo.Coeff_Vel_ = Coeff_Vel;

    std::vector<double> theta(4);
    std::vector<double> d_theta(4);
    std::vector<double> alpha(4);

    std::vector<double> theta_pso(4);
    std::vector<double> d_theta_pso(4);
    std::vector<double> alpha_pso(4);
    Matrix4d Laplace;
    for (int i = 0; i < 4; i++)
    {
        theta_pso[i]=theta[i] = i * M_PI / 2;
        d_theta_pso[i]=d_theta[i] = 0;
        alpha_pso[i]=alpha[i] = 0;
    }
    //main ctrl loop
    while (ros::ok())
    {
        /**
         * @brief TEST solution to lambda2 TIME
         */
        // int max_times = 50000;
        // ros::Time begin0 = ros::Time::now();
        // for (int i = 0; i < max_times; i++)
        // {
        //     double a = double(rand() % 600) / 1000;
        //     double b = double(rand() % 600) / 1000;
        //     double c = double(rand() % 600) / 1000;
        //     double d = double(rand() % 600) / 1000;
        //     double e = double(rand() % 600) / 1000;
        //     double f = double(rand() % 600) / 1000;

        //     Laplace << a + b + c, -a, -b, -c,
        //         -a, a + d + e, -d, -e,
        //         -b, -d, b + d + f, -f,
        //         -c, -e, -f, c + e + f;
        //     double lambda1 = SwarmOptiTopo.SolveLambda2_Analytical(Laplace);
        // }
        // ros::Time begin1 = ros::Time::now();

        // for (int i = 0; i < max_times;i++){
        //     double a = double(rand() % 600) / 1000;
        //     double b = double(rand() % 600) / 1000;
        //     double c = double(rand() % 600) / 1000;
        //     double d = double(rand() % 600) / 1000;
        //     double e = double(rand() % 600) / 1000;
        //     double f = double(rand() % 600) / 1000;

        //     Laplace << a + b + c, -a, -b, -c,
        //         -a, a + d + e, -d, -e,
        //         -b, -d, b + d + f, -f,
        //         -c, -e, -f, c + e + f;
        //     double lambda2 = SwarmOptiTopo.SolveLambda2_Eigen(Laplace);
        // }
        // ros::Time begin2 = ros::Time::now();

        // for (int i = 0; i < max_times;i++){
        //     double a = double(rand() % 600) / 1000;
        //     double b = double(rand() % 600) / 1000;
        //     double c = double(rand() % 600) / 1000;
        //     double d = double(rand() % 600) / 1000; // int max_times = 10000;
        //     double e = double(rand() % 600) / 1000;
        //     double f = double(rand() % 600) / 1000;

        //     Laplace << a + b + c, -a, -b, -c,
        //         -a, a + d + e, -d, -e,
        //         -b, -d, b + d + f, -f,
        //         -c, -e, -f, c + e + f;
        //     double lambda3 = SwarmOptiTopo.SolveLambda2_PowerIteration(Laplace,20,0.00001);
        // }
        // ros::Time begin3 = ros::Time::now();
        // ros::Duration dur1 = begin1 - begin0;
        // ros::Duration dur2 = begin2 - begin1;
        // ros::Duration dur3 = begin3 - begin2;
        // cout << "Analytical    :" << dur1.toSec() << endl;
        // cout << "Eigen         :" << dur2.toSec() << endl;
        // cout << "PowerIteration:" << dur3.toSec() << endl;
        // cout << "--------------------------" << endl; 


        /**
         * @brief TEST solution to lambda2 ACCRUACY
         */
        // double a = double(rand() % 600) / 1000;
        // double b = double(rand() % 600) / 1000;
        // double c = double(rand() % 600) / 1000;
        // double d = double(rand() % 600) / 1000;
        // double e = double(rand() % 600) / 1000;
        // double f = double(rand() % 600) / 1000;

        // Laplace << a + b + c, -a, -b, -c,
        //     -a, a + d + e, -d, -e,
        //     -b, -d, b + d + f, -f,
        //     -c, -e, -f, c + e + f;
        // double lambda1 = SwarmOptiTopo.SolveLambda2_Analytical(Laplace);
        // double lambda2 = SwarmOptiTopo.SolveLambda2_Eigen(Laplace);
        // double lambda3 = SwarmOptiTopo.SolveLambda2_PowerIteration(Laplace,20,0.00001);
        // cout << "Analytical    :" << lambda1 << endl;
        // cout << "Eigen         :" << lambda2 << endl;
        // cout << "PowerIteration:" << lambda3 << endl;
        // cout << "--------------------------" << endl;

        /**
         * @brief TEST solution to lambda2 ACCRUACY 随着 收敛门槛的变化
         */
        ofstream p;
        p.open("/home/up/workspaces/ws_2022/src/swarm_control/csv/output_1.csv",ios::out|ios::trunc);
        p << "reso"
          << ","
          << "acc"
          << ","
          << "time" << endl;
        int max_times = 10000;
        int reso = 1 + 500;
        double upper = -8.0;
        double lower = -2.0;
        vector<double> plot_x_reso(reso);
        vector<double> plot_y_acc(reso);

        ros::Time begin_time_0 = ros::Time::now();
        for (int j = 0; j < reso;j++){
            cout << "j:" << j << endl;
            double conver_thre = pow(10, lower + double(j) / (reso - 1) * (upper - lower));
            double abs_total = 0.0;
            for (int i = 0; i < max_times; i++)
            {
                double a = double(rand() % 600) / 1000;
                double b = double(rand() % 600) / 1000;
                double c = double(rand() % 600) / 1000;
                double d = double(rand() % 600) / 1000;
                double e = double(rand() % 600) / 1000;
                double f = double(rand() % 600) / 1000;

                Laplace << a + b + c, -a, -b, -c,
                    -a, a + d + e, -d, -e,
                    -b, -d, b + d + f, -f,
                    -c, -e, -f, c + e + f;
                
                double lambda1 = SwarmOptiTopo.SolveLambda2_Analytical(Laplace);
                double lambda3 = SwarmOptiTopo.SolveLambda2_PowerIteration(Laplace, 20, conver_thre);
                abs_total += abs(lambda1 - lambda3);
            }
            ros::Time begin_time_1 = ros::Time::now();
            for (int i = 0; i < max_times; i++)
            {
                double a = double(rand() % 600) / 1000;
                double b = double(rand() % 600) / 1000;
                double c = double(rand() % 600) / 1000;
                double d = double(rand() % 600) / 1000;
                double e = double(rand() % 600) / 1000;
                double f = double(rand() % 600) / 1000;

                Laplace << a + b + c, -a, -b, -c,
                    -a, a + d + e, -d, -e,
                    -b, -d, b + d + f, -f,
                    -c, -e, -f, c + e + f;
                
                double lambda1 = SwarmOptiTopo.SolveLambda2_Analytical(Laplace);
            }
            ros::Time begin_time_2 = ros::Time::now();
            ros::Duration dur1 = begin_time_1 - begin_time_0;
            ros::Duration dur2 = begin_time_2 - begin_time_1;

            p << conver_thre
              << ","
              << abs_total / max_times 
              << "," 
              << (dur1.toSec() - dur2.toSec()) / dur2.toSec() << endl;
        }
        p.close();
        break;


        /**
         * @brief TEST the process of iteration
         */
        // ros::Duration duration = ros::Time::now() - PID_begin_time;
        // double t_gap = duration.toSec();
        // double delta_t = t_gap - last_t_gap;
        // cout << "delta_t" << delta_t << endl;
        // last_t_gap = t_gap;

        // SwarmOptiTopo.GetOutput_search(theta, d_theta, alpha, 0.03333);
        // // SwarmOptiTopo.GetOutput_pso(theta, d_theta, alpha, 0.03333);
        // for (int i = 0; i < 4; i++)
        // {
        //     theta[i] = SwarmOptiTopo.theta_out(i);
        //     d_theta[i] = SwarmOptiTopo.d_theta_out(i);
        //     alpha[i] = SwarmOptiTopo.alpha_out(i);
        //     // cout << "theta" << theta[i] << endl;
        //     // cout <<"d_theta"<< d_theta[i] << endl;
        //     // cout << "alpha" << alpha[i] << endl;
        // }
        // exp_pose_1.pose.position.x = theta[1];
        // exp_pose_1.pose.orientation.w = alpha[1];
        // exp_pose_1.pose.position.y = d_theta[1];
        // exp_pose_1.pose.position.z = SwarmOptiTopo.dd_theta_out(1);

        // exp_pose_2.pose.position.x = theta[2];
        // exp_pose_2.pose.orientation.w = alpha[2];
        // exp_pose_2.pose.position.y = d_theta[2];
        // exp_pose_2.pose.position.z = SwarmOptiTopo.dd_theta_out(2);

        // exp_pose_3.pose.position.x = theta[3];
        // exp_pose_3.pose.orientation.w = alpha[3];
        // exp_pose_3.pose.position.y = d_theta[3];
        // exp_pose_3.pose.position.z = SwarmOptiTopo.dd_theta_out(3);

        // exp_pose_4.pose.position.x = theta[0];
        // exp_pose_4.pose.orientation.w = alpha[0];
        // exp_pose_4.pose.position.y = d_theta[0];
        // exp_pose_4.pose.position.z = SwarmOptiTopo.dd_theta_out(0);

        // exp_pose_1.header.stamp = exp_pose_2.header.stamp = exp_pose_3.header.stamp = exp_pose_4.header.stamp = ros::Time::now();
        // uav1_pub.publish(exp_pose_1);
        // uav2_pub.publish(exp_pose_2);
        // uav3_pub.publish(exp_pose_3);
        // uav4_pub.publish(exp_pose_4);
        // cout << "-------------------------------" << endl;

        /**
         * @brief Compare the process of iteration between search and pso
         */
        // ros::Duration duration = ros::Time::now() - PID_begin_time;
        // double t_gap = duration.toSec();
        // double delta_t = t_gap - last_t_gap;
        // cout << "delta_t" << delta_t << endl;
        // last_t_gap = t_gap;

        // SwarmOptiTopo.GetOutput_pso(theta_pso, d_theta_pso, alpha_pso, 0.0333);
        // for (int i = 0; i < 4; i++)
        // {
        //     theta_pso[i] = SwarmOptiTopo.theta_out(i);
        //     d_theta_pso[i] = SwarmOptiTopo.d_theta_out(i);
        //     alpha_pso[i] = SwarmOptiTopo.alpha_out(i);
        // }
        // exp_pose_1.pose.position.x = theta_pso[1];
        // exp_pose_1.pose.orientation.w = alpha_pso[1];
        // exp_pose_1.pose.position.y = d_theta_pso[1];
        // exp_pose_1.pose.position.z = SwarmOptiTopo.dd_theta_out(1);

        // exp_pose_2.pose.position.x = theta_pso[2];
        // exp_pose_2.pose.orientation.w = alpha_pso[2];
        // exp_pose_2.pose.position.y = d_theta_pso[2];
        // exp_pose_2.pose.position.z = SwarmOptiTopo.dd_theta_out(2);


        // SwarmOptiTopo.GetOutput_search(theta, d_theta, alpha, 0.0333);
        // for (int i = 0; i < 4; i++)
        // {
        //     theta[i] = SwarmOptiTopo.theta_out(i);
        //     d_theta[i] = SwarmOptiTopo.d_theta_out(i);
        //     alpha[i] = SwarmOptiTopo.alpha_out(i);
        // }
        // exp_pose_3.pose.position.x = theta[1];
        // exp_pose_3.pose.orientation.w = alpha[1];
        // exp_pose_3.pose.position.y = d_theta[1];
        // exp_pose_3.pose.position.z = SwarmOptiTopo.dd_theta_out(1);

        // exp_pose_4.pose.position.x = theta[2];
        // exp_pose_4.pose.orientation.w = alpha[2];
        // exp_pose_4.pose.position.y = d_theta[2];
        // exp_pose_4.pose.position.z = SwarmOptiTopo.dd_theta_out(2);

        // exp_pose_1.header.stamp = exp_pose_2.header.stamp = exp_pose_3.header.stamp = exp_pose_4.header.stamp = ros::Time::now();
        // uav1_pub.publish(exp_pose_1);
        // uav2_pub.publish(exp_pose_2);
        // uav3_pub.publish(exp_pose_3);
        // uav4_pub.publish(exp_pose_4);

        // // cout <<"theta"<< theta[0] << endl;
        // // cout <<"d_theta"<< d_theta[0] << endl;
        // // cout <<"alpha"<< alpha[0] << endl;
        // cout << "-------------------------------" << endl;


        /**
         * @brief IMPROVE the PSO and PLOT
         */
        // ros::Duration duration = ros::Time::now() - PID_begin_time;
        // double t_gap = duration.toSec();
        // double delta_t = t_gap - last_t_gap;
        // cout << "delta_t" << delta_t << endl;
        // last_t_gap = t_gap;

        // // SwarmOptiTopo.GetOutput_search(theta, d_theta, alpha, delta_t);
        // SwarmOptiTopo.GetOutput_pso(theta, d_theta, alpha, 0.0333);
        // vector<double> plot_x_cam(pso_max_iter_num);
        // vector<double> plot_x_uav(pso_max_iter_num);
        
        // for (int i = 0; i < pso_max_iter_num; i++){
        //     plot_x_cam[i] = SwarmOptiTopo.cam_results_[i];
        //     plot_x_uav[i] = SwarmOptiTopo.uav_results_[i];
        // }
        // plt::clf();
        // plt::named_plot("camera", plot_x_cam);
        // plt::named_plot("uav",plot_x_uav);
        // plt::legend();
        // for (int i = 0; i < 4; i++)
        // {
        //     theta[i] = SwarmOptiTopo.theta_out(i);
        //     d_theta[i] = SwarmOptiTopo.d_theta_out(i);
        //     alpha[i] = SwarmOptiTopo.alpha_out(i);
        // }
        // cout << "theta   :" << theta[0] << " " << theta[1] << " " << theta[2] << " " << theta[3] << endl;
        // cout << "d_theta :" << d_theta[0] << " " << d_theta[1] << " " << d_theta[2] << " " << d_theta[3] << endl;
        // cout << "alpha   :" << alpha[0] << " " << alpha[1] << " " << alpha[2] << " " << alpha[3] << endl;
        // plt::show();


        // exp_pose_1.pose.position.x = theta[1];
        // exp_pose_1.pose.orientation.w = alpha[1];
        // exp_pose_1.pose.position.y = d_theta[1];
        // exp_pose_1.pose.position.z = SwarmOptiTopo.dd_theta_out(1);

        // exp_pose_2.pose.position.x = theta[2];
        // exp_pose_2.pose.orientation.w = alpha[2];
        // exp_pose_2.pose.position.y = d_theta[2];
        // exp_pose_2.pose.position.z = SwarmOptiTopo.dd_theta_out(2);

        // exp_pose_3.pose.position.x = theta[3];
        // exp_pose_3.pose.orientation.w = alpha[3];
        // exp_pose_3.pose.position.y = d_theta[3];
        // exp_pose_3.pose.position.z = SwarmOptiTopo.dd_theta_out(3);

        // exp_pose_4.pose.position.x = theta[0];
        // exp_pose_4.pose.orientation.w = alpha[0];
        // exp_pose_4.pose.position.y = d_theta[0];
        // exp_pose_4.pose.position.z = SwarmOptiTopo.dd_theta_out(0);

        // exp_pose_1.header.stamp = exp_pose_2.header.stamp = exp_pose_3.header.stamp = exp_pose_4.header.stamp = ros::Time::now();
        // uav1_pub.publish(exp_pose_1);
        // uav2_pub.publish(exp_pose_2);
        // uav3_pub.publish(exp_pose_3);
        // uav4_pub.publish(exp_pose_4);
        // cout << "-------------------------------" << endl;


        // ctrl_loop.sleep();
    }

    

    return 0;
}